#include "Robot.h"

using namespace std;
Pos getNextPos(Pos pos, int i);

bool Robot::accurateFindPath(Map map, Pos targetPos, Pos &returnPos){
    fill(visit[0], visit[0] + sizeof(visit), false);
    Pos allPos[MAX_MAP_SIZE][MAX_MAP_SIZE];
    for(int i = 0; i < MAX_MAP_SIZE; ++i){
        for(int j = 0; j < MAX_MAP_SIZE; ++j){
            allPos[i][j].x = i;
            allPos[i][j].y = j;
        }
    }
    Pos root = pos;
    root.step = 0;
    queue<Pos*>que;
    que.push(&root);
    visit[root.x][root.y] = true;
    while(!que.empty()){
        Pos pos = *que.front();
        if(pos.equal(targetPos)){
            vector<Pos>path;
            path.push_back(pos);
            Pos *p = &pos;
            p = p->pre;
            while(p != NULL){
                path.push_back(*p);
                p = p->pre;
            }
            returnPos = path[1];
            return true;
        }
        que.pop();
        for(int i = 1; i <= 9; ++i){
            if(map.getChessNode(pos).units.front() == WALL && i == pos.direction) continue;
            Pos &nextPos = allPos[getNextPos(pos, i).x][getNextPos(pos, i).y];
            nextPos.direction = i;
            if(map.getChessNode(nextPos).units.front() == WALL){
                Pos &wallBehindPos = allPos[getNextPos(nextPos, i).x][getNextPos(nextPos, i).y];
                if(map.getChessNode(wallBehindPos).units.front() == BLANK){
                    visit[nextPos.x][nextPos.y] = true;
                    nextPos.step = pos.step + 1;
                    nextPos.pre = &pos;
                    que.push(&nextPos);
                }
            }else if(map.getChessNode(nextPos).units.front() == RACK ||
                    map.getChessNode(nextPos).units.front() == SERVER ||
                    map.getChessNode(nextPos).units.front() == POWER ||
                    map.getChessNode(nextPos).units.front() == BLANK){
                visit[nextPos.x][nextPos.y] = true;
                nextPos.step = pos.step + 1;
                nextPos.pre = &pos;
                que.push(&nextPos);
            }
        }
    }
    return false;
}

Pos getNextPos(Pos pos, int i){
    Pos ret;
    if(i == 1){
        ret.x = pos.x + 1;
        ret.y = pos.y - 1;
    }else if(i == 2){
        ret.x = pos.x + 1;
        ret.y = pos.y - 1;
    }else if(i == 3){
    }else if(i == 4){
    }else if(i == 5){
        return pos;
    }else if(i == 6){
    }else if(i == 7){
    }else if(i == 8){
    }else if(i == 9){

    }
    return ret;
}

int Robot::getMaxStep(Map map){
    fill(visit[0], visit[0] + sizeof(visit), false);
    Pos root = pos;
    root.step = 0;
    queue<Pos>que;
    que.push(root);
    visit[root.x][root.y] = true;
    int maxStep = -1;
    while(!que.empty()){
        Pos pos = que.front();
        maxStep = max(pos.step, maxStep);
        que.pop();
        for(int i = 1; i <= 9; ++i){
            if(map.getChessNode(pos).units.front() == WALL && i == pos.direction) continue;
            Pos nextPos = getNextPos(pos, i);
            nextPos.direction = i;
            nextPos.step = pos.step + 1;
            if(map.getChessNode(nextPos).units.front() == WALL){
                Pos wallBehindPos = getNextPos(nextPos, i);
                if(map.getChessNode(wallBehindPos).units.front() == BLANK){
                    visit[nextPos.x][nextPos.y] = true;
                    que.push(nextPos);
                }
            }else if(map.getChessNode(nextPos).units.front() == RACK ||
                    map.getChessNode(nextPos).units.front() == SERVER ||
                    map.getChessNode(nextPos).units.front() == POWER ||
                    map.getChessNode(nextPos).units.front() == BLANK){
                visit[nextPos.x][nextPos.y] = true;
                que.push(nextPos);
            }
        }
    }
    return maxStep;
}













